/**
  ******************************************************************************
  * @file   Service_Devices.c
  * @brief  Devices service running file.
  ******************************************************************************
  * @note
  *  - Before running your devices, just do what you want ~ !
  *  - More devices or using other classification is decided by yourself ~ !
  ===============================================================================
                                    Task List
  ===============================================================================
  * <table>
  * <tr><th>Task Name     <th>Priority          <th>Frequency/Hz    <th>Stack/Byte
  * <tr><td>              <td>                  <td>                <td>    
  * </table>
  *
 */
/* Includes ------------------------------------------------------------------*/
#include "SRML.h"
#include "Service_Devices.h"

#include "App_Chassis.h"
/* Private define ------------------------------------------------------------*/
TaskHandle_t DeviceActuators_Handle;
TaskHandle_t DeviceDR16_Handle;
TaskHandle_t DeviceSensors_Handle;
TaskHandle_t DevicePC_Handle;


/* Private variables ---------------------------------------------------------*/
/* Private function declarations ---------------------------------------------*/
void Device_Actuators(void *arg);
void Device_Sensors(void *arg);
void Device_DR16(void *arg);
void Device_PC(void *arg);

/* Exported devices ----------------------------------------------------------*/
/* Motor & ESC & Other actuators*/

/* Remote control */

/* IMU & NUC & Other sensors */

/* Other boards */

/* Function prototypes -------------------------------------------------------*/
/**
* @brief  Initialization of device management service
* @param  None.
* @return None.
*/
void Service_Devices_Init(void)
{
  xTaskCreate(Device_Actuators, "Dev.Actuator" , Normal_Stack_Size,    NULL, PriorityHigh,   &DeviceActuators_Handle);
	//xTaskCreate(Device_PC, "Dev.PC" , Tiny_Stack_Size,    NULL, PriorityHigh,   &DevicePC_Handle);
  //xTaskCreate(Device_DR16,      "Dev.DR16"     , Tiny_Stack_Size,    NULL, PriorityHigh,        &DeviceDR16_Handle);
  xTaskCreate(Device_Sensors,   "Dev.Sensors"  , Huge_Stack_Size,    NULL, PrioritySuperHigh,        &DeviceSensors_Handle);
}

void Device_Actuators(void *arg)
{
  /* Cache for Task */

  /* Pre-Load for task */

  /* Infinite loop */
  TickType_t xLastWakeTime_t;
  xLastWakeTime_t = xTaskGetTickCount();
  for(;;)
  {
    /* Connection check */
		
    /* Read Message */
		
    /* Send Message */
		Chassis_Adjust();
		Chassis_Transmit_HERO(&huart6);
    /* Pass control to the next task */
    vTaskDelayUntil(&xLastWakeTime_t,1);
  }
}

void Device_DR16(void *arg)
{
  /* Cache for Task */
\
  /* Pre-Load for task */
	
  /* Infinite loop \*/
  TickType_t xLastWakeTime_t;
  xLastWakeTime_t = xTaskGetTickCount();
	
  for(;;)
  {
    vTaskDelayUntil(&xLastWakeTime_t,1000);
  }
}

void Device_Sensors(void *arg)
{
  /* Cache for Task */

  /* Pre-Load for task */
	//MPU6050_Init(GPIOB, GPIO_PIN_6, GPIO_PIN_7);
  /* Infinite loop */
  TickType_t xLastWakeTime_t;
  xLastWakeTime_t = xTaskGetTickCount();
  for(;;)
  {
    /* Read IMU Message */
		//Chassis_World_Angle();
    /* Exchange NUC Meaasge */
    /* Read Other board Message */
  /* Pass control to the next task ------------------------------------------*/
    vTaskDelayUntil(&xLastWakeTime_t,5);
  }
}

void Device_PC(void *arg)
{
  TickType_t xLastWakeTime_t;
  xLastWakeTime_t = xTaskGetTickCount();
  for(;;)
  {
    vTaskDelayUntil(&xLastWakeTime_t,100);
  }
}


/* User Code End Here ---------------------------------*/

/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
